/*
   This example code is in the Public Domain (or CC0 licensed, at your option.)

   Unless required by applicable law or agreed to in writing, this
   software is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
   CONDITIONS OF ANY KIND, either express or implied.
*/

#include <stdio.h>
#include <string.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/event_groups.h"
#include "esp_bt.h"
#include "esp_log.h"
#include "nvs_flash.h"

#include "esp_bt_defs.h"
#include "esp_bt_main.h"
#include "esp_bt_device.h"
#include "esp_gatt_defs.h"
#include "esp_gattc_api.h"
#include "esp_gap_ble_api.h"

#include "feather_esp.h"

static const char *tag = "FEATHER_ESP32";
static const char bin2hex[] = {'0','1','2','3','4','5','6','7','8','9','A','B','C','D','E','F',0};
char dev_name[12] = {0};

#ifdef USE_BLE_ADV  
static esp_ble_adv_params_t ble_adv_params = {
    .adv_int_min        = 0x20,
    .adv_int_max        = 0x40,
    .adv_type           = ADV_TYPE_NONCONN_IND,
    .own_addr_type      = BLE_ADDR_TYPE_PUBLIC,
    .channel_map        = ADV_CHNL_ALL,
    .adv_filter_policy = ADV_FILTER_ALLOW_SCAN_ANY_CON_ANY,
};
#endif

#ifdef USE_BLE_SCAN 
static esp_ble_scan_params_t ble_scan_params = {
    .scan_type              = BLE_SCAN_TYPE_ACTIVE,
    .own_addr_type          = BLE_ADDR_TYPE_PUBLIC,
    .scan_filter_policy     = BLE_SCAN_FILTER_ALLOW_ALL,
    .scan_interval          = 0x50,
    .scan_window            = 0x30,
    .scan_duplicate         = BLE_SCAN_DUPLICATE_DISABLE
};
#endif

#ifdef USE_BLE_ADV  
static void ble_set_adv_data(uint8_t *data,uint8_t len)
{
    char *adv_name = "Feather";
    uint8_t name_len = (uint8_t)strlen(adv_name);
    uint8_t adv_data[31] = {0x02, 0x01, 0x06, 0x0, 0x09,};
    uint8_t adv_data_len;
    uint8_t *ble_mac;

    adv_data[3] = name_len + 1;
    for (int i = 0; i < name_len; i++) 
    {
        adv_data[5 + i] = (uint8_t)adv_name[i];
    }
    adv_data_len = 5 + name_len;

    if(ESP_BLUEDROID_STATUS_ENABLED != esp_bluedroid_get_status())
    {
        esp_bluedroid_enable();
    }
    if(ESP_BLUEDROID_STATUS_ENABLED == esp_bluedroid_get_status())
    {
        ble_mac = (uint8_t *)esp_bt_dev_get_address();
        ESP_LOGI(tag, "ble_mac: %02X:%02X:%02X:%02X:%02X:%02X", ble_mac[0],ble_mac[1],ble_mac[2],ble_mac[3],ble_mac[4],ble_mac[5]);

        adv_data[adv_data_len    ] = bin2hex[((ble_mac[4]>>4)&0x0f)];
        adv_data[adv_data_len + 1] = bin2hex[((ble_mac[4])&0x0f)];
        adv_data[adv_data_len + 2] = bin2hex[((ble_mac[5]>>4)&0x0f)];
        adv_data[adv_data_len + 3] = bin2hex[((ble_mac[5])&0x0f)];
    }
    else
    {
        adv_data[adv_data_len    ] = '0';
        adv_data[adv_data_len + 1] = '0';
        adv_data[adv_data_len + 2] = '0';
        adv_data[adv_data_len + 3] = '0';
    }
    adv_data[3] += 4;
    adv_data_len += 4;

    adv_data[adv_data_len    ] = ((30 - adv_data_len) > (len+3) ? (len+3) : (30 - adv_data_len));  //以下数据长度
    adv_data[adv_data_len + 1] = 0xFF;  //数据类型:自定义类型
    adv_data[adv_data_len + 2] = 0xE5;  //厂商ID L
    adv_data[adv_data_len + 3] = 0x02;  //厂商ID H
    adv_data_len += 4;

    for(int i = 0;i < ((31 - adv_data_len) > len ? len : (31 - adv_data_len));i ++)
    {
        adv_data[adv_data_len + i] = data[i]; 
    }

    adv_data_len += ((31 - adv_data_len) > len ? len : (31 - adv_data_len));

    strncpy(dev_name,(char *)&adv_data[5],11);
    esp_ble_gap_config_adv_data_raw(adv_data, adv_data_len);
}
#endif

void parse_ctrl_data(uint8_t *data,uint8_t len)
{
    uint8_t index = 0;
    while(index < len)
    {
        uint8_t field_length = data[index];
        uint8_t field_type = data[index+1];
        if(field_type == 0xFF)
        {
            if(strstr((char *)data,dev_name)) 
            {
#ifdef USE_MOT
                char *mp = strstr((char *)data,"-M");
                if(mp != NULL && len >= 29)
                {
                    mp += 2;
                    float duty = ((mp[0]-'0')*100 + (mp[1]-'0')*10 + (mp[2]-'0')) % 101;
                    mot_duty[0] = duty*MOT_DUTY_FULL_AERO/100.0+MOT_DUTY_DEAD_AERO;
                    mot_sata[0] = 1;
                    mp += 3;

                    duty = ((mp[0]-'0')*100 + (mp[1]-'0')*10 + (mp[2]-'0')) % 101;
                    mot_duty[1] = duty*MOT_DUTY_FULL_AERO/100.0+MOT_DUTY_DEAD_AERO;
                    mot_sata[1] = 1;
                    mp += 3;

                    duty = ((mp[0]-'0')*100 + (mp[1]-'0')*10 + (mp[2]-'0')) % 101;
                    mot_duty[2] = duty*MOT_DUTY_FULL_AERO/100.0+MOT_DUTY_DEAD_AERO;
                    mot_sata[2] = 1;
                    mp += 3;

                    duty = ((mp[0]-'0')*100 + (mp[1]-'0')*10 + (mp[2]-'0')) % 101;
                    mot_duty[3] = duty*MOT_DUTY_FULL_AERO/100.0+MOT_DUTY_DEAD_AERO;
                    mot_sata[3] = 1;
                    
                    printf("Set MOT duty to: %d, %d, %d, %d\r\n",mot_duty[0],mot_duty[1],mot_duty[2],mot_duty[3]);
                }
#endif
            }
        }
        index = index + field_length + 1;
    }
}

static void esp_gap_cb(esp_gap_ble_cb_event_t event, esp_ble_gap_cb_param_t* param)
{
    esp_err_t err;

    switch(event)
    {
        case ESP_GAP_BLE_ADV_DATA_RAW_SET_COMPLETE_EVT:
        {
#ifdef USE_BLE_ADV
            esp_ble_gap_start_advertising(&ble_adv_params);
#endif
            break;
        }
        case ESP_GAP_BLE_SCAN_PARAM_SET_COMPLETE_EVT: 
        {
#ifdef USE_BLE_SCAN
            uint32_t duration = 0;
            esp_ble_gap_start_scanning(duration);
#endif
            break;
        }
        case ESP_GAP_BLE_ADV_START_COMPLETE_EVT:
            //adv start complete event to indicate adv start successfully or failed
            if ((err = param->adv_start_cmpl.status) != ESP_BT_STATUS_SUCCESS) 
            {
                ESP_LOGE(tag, "Adv start failed: %s", esp_err_to_name(err));
            }
            else 
            {
                ESP_LOGI(tag,"Advertising...");
            }
        break;
        case ESP_GAP_BLE_SCAN_START_COMPLETE_EVT: 
        {
            if((err = param->scan_start_cmpl.status) != ESP_BT_STATUS_SUCCESS) 
            {
                ESP_LOGE(tag,"Scan start failed: %s", esp_err_to_name(err));
            }
            else 
            {
                ESP_LOGI(tag,"Start scanning...");
            }
            break;
        }
        case ESP_GAP_BLE_SCAN_RESULT_EVT: 
        {
            int i = 0;
            printf("ble scan: ");
            for(i = 0;i < (param->scan_rst.adv_data_len+param->scan_rst.scan_rsp_len);i ++)
            {
                printf("%02X",param->scan_rst.ble_adv[i] & 0xff);
            }
            printf(":%d:%d:%d\r\n",param->scan_rst.num_resps,param->scan_rst.adv_data_len,param->scan_rst.scan_rsp_len);
            parse_ctrl_data(param->scan_rst.ble_adv,i);
            break;
        }
        case ESP_GAP_BLE_ADV_STOP_COMPLETE_EVT:
        {
            if ((err = param->adv_stop_cmpl.status) != ESP_BT_STATUS_SUCCESS)
            {
                ESP_LOGE(tag, "Adv stop failed: %s", esp_err_to_name(err));
            }
            else 
            {
                ESP_LOGI(tag, "Stop adv successfully");
            }
            break;
        }
        case ESP_GAP_BLE_SCAN_STOP_COMPLETE_EVT:
        {
            if((err = param->scan_stop_cmpl.status) != ESP_BT_STATUS_SUCCESS) 
            {
                ESP_LOGE(tag,"Scan stop failed: %s", esp_err_to_name(err));
            }
            else 
            {
                ESP_LOGI(tag,"Stop scan successfully");
            }
            break;
        }
        default:
            break;
    }
}

void ble_init(void)
{
    esp_err_t status;
    esp_bluedroid_init();
    esp_bluedroid_enable();
    ESP_LOGI(tag,"Register callback");
    /*<! register the scan callback function to the gap module */
    if((status = esp_ble_gap_register_callback(esp_gap_cb)) != ESP_OK) 
    {
        ESP_LOGE(tag,"gap register error: %s", esp_err_to_name(status));
        return;
    }

#ifdef USE_BLE_ADV 
    const uint8_t adv_data[11] = {0,1,2,3,4,5,6,7,8,9,0};
    ble_set_adv_data((uint8_t *)adv_data,11);
#endif

#ifdef USE_BLE_SCAN 
    esp_ble_gap_set_scan_params(&ble_scan_params);
#endif
}


